/******************************************************************************
*
* Copyright (C) 2009 - 2014 Xilinx, Inc.  All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* Use of the Software is limited solely to applications:
* (a) running on a Xilinx device, or
* (b) that interact with a Xilinx device through a bus or interconnect.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* XILINX CONSORTIUM BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* Except as contained in this notice, the name of the Xilinx shall not be used
* in advertising or otherwise to promote the sale, use or other dealings in
* this Software without prior written authorization from Xilinx.
*
******************************************************************************/

/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

#include <stdio.h>
#include "platform.h"
#include "xparameters.h"
#include "sleep.h"
#include "zrcar.h"
#define MOVE_FORWARD    1500
#define MOVE_BACK       100
#define DANGEROUS_DIS   100
#define MAX_DIS         1000000
#define MOVE_SLEEP      964800
#define TURN_SLEEP      400000
#define TURN_SPEED      16
float ult_data[3] = {0,0,0};

int car_x = 0;
int car_y = 0;
int face = 0;


void car_stop()
{
    zrcar_wheel_l_set(0);
    zrcar_wheel_r_set(0);
}
void car_forward()
{
    zrcar_wheel_l_set(-8);
    zrcar_wheel_r_set(-8);
}
void car_back()
{
    zrcar_wheel_l_set(4);
    zrcar_wheel_r_set(4);
}
void car_right()
{
    zrcar_wheel_l_set(0);
    zrcar_wheel_r_set(-TURN_SPEED);
}
void car_turn_r()
{
    zrcar_wheel_r_set(TURN_SPEED);
    zrcar_wheel_l_set(-TURN_SPEED);
}
void car_left()
{
    zrcar_wheel_l_set(-TURN_SPEED);
    zrcar_wheel_r_set(0);
}
void car_turn_l()
{
    zrcar_wheel_l_set(TURN_SPEED);
    zrcar_wheel_r_set(-TURN_SPEED);
}

int test_head(int next_head){
    printf("start test\r\n");

    int check = 2;
    while(check){
        zrcar_ultra_get_all(ult_data);
        printf("start ult_data: %f\r\n",ult_data[1]);
        if(ult_data[1] < DANGEROUS_DIS || ult_data[1] > MAX_DIS){
            xil_printf("start turn\r\n");
            check --;
            if(next_head > face){
                car_turn_l();
                usleep(MOVE_SLEEP);
                car_stop();
            }
            else{
                car_turn_r();
                usleep(MOVE_SLEEP);
                car_stop();
            }
            face = next_head;
        }
        else{
            switch(face){
                case 0:
                    car_x += 1;
                    break;
                case 1:
                    car_y += 1;
                    break;
                case 2:
                    car_x -= 1;
                    break;
                case 3:
                    car_y -= 1;
                    break;
            }
            car_forward();
            usleep(TURN_SLEEP);
            car_stop();
            break;
        }
    }
    return 0;
}
int find_path(int total_x,int total_y){

    int head = 0;
    int next_head = 0;
    int turn = 0;
    int x = total_x - car_x;
    int y = total_y - car_y;
    if(!(x || y)){
        return 1;
    }
    int head_x = (x < 0) ? 2 : 0;
    int head_y = (y < 0) ? 3 : 1;

    if( x*x - y*y < 0){
        head = head_y;
        next_head = head_x;
    }
    else{
        head = head_x;
        next_head = head_y;
    }
    turn = head - face;
    switch (turn){
        case 0:
            break;
        case 1:
            car_turn_l();
            usleep(MOVE_SLEEP);
            car_stop();
            break;
        case 2:
            car_turn_l();
            usleep(2*MOVE_SLEEP);
            car_stop();
            break;
        case 3:
            car_turn_l();
            usleep(3*MOVE_SLEEP);
            car_stop();
            break;
        case -1:
            car_turn_r();
            usleep(MOVE_SLEEP);
            car_stop();
            break;
        case -2:
            car_turn_r();
            usleep(2*MOVE_SLEEP);
            car_stop();
            break;
        case -3:
            car_turn_r();
            usleep(3*MOVE_SLEEP);
            car_stop();
            break;
    }
    face = head;
    test_head(next_head);
    return 0;
}

int main()
{
    zrcar_wheel_init();
    zrcar_ultra_init();


    //zrcar_bluetooth_init();
    xil_printf("start\r\n");
    int i;
    for(i = 1; i < 4; i++){
        xil_printf("u%d : %f mm ",i , ult_data[i-1]);
    }
    while(!find_path(15,20)){

        xil_printf("location: ( %d, %d)\r\n", car_x,car_y);
    }
    xil_printf("finish\r\n");
    return 0;
}
